%利用matlab2opencv函数从matlab标定结果矩阵中提取所需的数值保存到对应的yml文件中(按opencv函数所需矩阵的顺序设置值),以供opencv直接利用cvLoad等加载调用.

% cd('/home/daybeha/Documents/diablo_ws/two-wheel-leg-robot/calibration/inu_calib/camera');;%切换到立体标定结果的目录下
load stereoParams.mat;%加载标定结果矩阵

% .m文件，保存参数到yaml，注意相机相对位姿取逆
height = stereoParams.CameraParameters1.ImageSize(1);
width = stereoParams.CameraParameters1.ImageSize(2);
RD1 = stereoParams.CameraParameters1.RadialDistortion;
TD1 = stereoParams.CameraParameters1.TangentialDistortion;
D1 = [RD1(1), RD1(2), TD1(1), TD1(2)];
% D1 = [RD1(1), RD1(2), TD1(1), TD1(2), RD1(3)];
K1 = stereoParams.CameraParameters1.IntrinsicMatrix;
% K1 = K1(:);

% height2 = stereoParams.CameraParameters2.ImageSize(1);
% width2 = stereoParams.CameraParameters2.ImageSize(2);
RD2 = stereoParams.CameraParameters2.RadialDistortion;
TD2 = stereoParams.CameraParameters2.TangentialDistortion;
D2 = [RD2(1), RD2(2), TD2(1), TD2(2)];
% D2 = [RD2(1), RD2(2), TD2(1), TD2(2), RD2(3)];
K2 = stereoParams.CameraParameters2.IntrinsicMatrix;
% K2 = K2(:);

rot = stereoParams.RotationOfCamera2;
trans = stereoParams.TranslationOfCamera2/1000;
%取逆
T=eye(4);
T(1:3,1:3)=rot;
T(1:3,4)=trans;
T=inv(T);
rot=T(1:3,1:3);
trans = trans';
% rot = rot(:);
% trans=T(1:3,4);

T = T';
% T = T(:);

fx = stereoParams.CameraParameters2.IntrinsicMatrix(1,1);
baseline = norm(stereoParams.TranslationOfCamera2)/1000;    % 单位:m
bf = fx*baseline;



fileName = 'mystereo.yaml';
file = fopen( fileName, 'w');%不存在则创建写入模式
fprintf( file, '%%YAML:1.0\n');

%调用matlab2opencv函数保存矩阵到yml文件
matlab2opencv(width,fileName);%注意该函数脚本要与本脚本在同一目录下或者该函数脚本已设置路径了
matlab2opencv(height,fileName);
matlab2opencv(K1,fileName);
matlab2opencv(D1,fileName);
matlab2opencv(K2,fileName);
matlab2opencv(D2,fileName);
matlab2opencv(rot,fileName);
matlab2opencv(trans,fileName);
matlab2opencv(T,fileName);
matlab2opencv(baseline,fileName);
matlab2opencv(bf,fileName);
